// Takes an object, a list of real-valued positions, and
// speeds, and modifies the pose of the object to follow
// the path every time update() is called.
//
// Nick Kitten
// 2009

#include "PathTraverser.h"
#include <algorithm>
#include "DarkGDK.h"
#include <cassert>

using namespace std;

PathTraverser::PathTraverser()
{
}

PathTraverser::PathTraverser( const list<Triple> * path, float walkSpeed, float turnSpeed)
: path( path ), walkSpeed( walkSpeed ), turnSpeed( turnSpeed )
{
	//setPath( path, Triple() );
}

void PathTraverser::setPath( const list<Triple> * path, Triple currRot )
{
	this->path = path;
	this->poses.clear();
	this->poseTimes.clear();

	if( path->empty() )
		return;

	startTime = dbTimer();

	Pose pathPose;
	int dt, currTime = startTime;
	float lastDir, currDir;
	Triple rot;
	
	float dx, dz, dr;
	list<Triple>::const_iterator currPos = path->begin();
	list<Triple>::const_iterator nextPos = currPos;
	nextPos++;

	pathPose.position = *currPos;
	lastDir = currRot.y;
	pathPose.rotation = Triple( 0.0f, lastDir, 0.0f );
	poses.push_back( pathPose );

	poseTimes.push_back( currTime );

	while( nextPos != path->end() )
	{
		dx = nextPos->x - currPos->x;
		dz = nextPos->z - currPos->z;

		// computes sign-adjusted arctan( dx/dz ), yRot
		currDir  = dbATANFULL( dx, dz );
		rot = Triple( 0.0f, currDir, 0.0f );
		dr = fabs( currDir - lastDir );
		// Check if its faster to move clockwise
		if( dr > 180 )
			dr = 360 - dr;

		lastDir = currDir;

		// rotate from old orientation to face next node
		pathPose.position = *currPos;
		pathPose.rotation = rot;
		poses.push_back( pathPose );

		// calculate time it takes to rotate and store
		dt = (int)(dr / turnSpeed);
		currTime += dt;
		poseTimes.push_back( currTime );

		// for next leg, keep same rotation and move to next position
		pathPose.position = *nextPos;
		poses.push_back( pathPose );

		// calculate time 
		dt = (int)( getDistance( *currPos, *nextPos ) / walkSpeed );
		currTime += dt;
		poseTimes.push_back( currTime );

		// move to next leg in path
		nextPos++;
		currPos++;
	}
}

// Move object along the path according to the amount of time that's passed
Pose PathTraverser::update()
{
	now = dbTimer();
	int dt;
	int indOffset;
	float interpTime;
	Pose newPose, before, after;
	Triple pos, rot;
	vector<int>::iterator pTime;
	pTime = lower_bound( poseTimes.begin(), poseTimes.end(), now );
	indOffset = pTime - poseTimes.begin();
	
	// object is done following path.  Should change object state here
	if( pTime == poseTimes.end() )
	{
		newPose = poses.back();
	}
	else
	{
		// interpolate pose based on the current time location in the path
		dt = *pTime - *( pTime - 1 );
		interpTime = (float)( now - *( pTime - 1 ) ) / dt;
		before = poses[indOffset - 1];
		after = poses[indOffset];
		newPose = interpolate( before, after, interpTime );
	}

	return newPose;
}

void PathTraverser::setTurnSpeed(float newSpeed)
{
	this->turnSpeed = newSpeed;
}

void PathTraverser::setWalkSpeed( float newSpeed )
{
	this->walkSpeed = newSpeed;
}